#include "BC_FootTask.hpp"

template <typename T>
void  FootTask<T>::task(ControlFSMData<T>& data, BalanceCtrlData<T>* ctrl_data, BCoutputData<T>* BCout, int leg){
    auto& seResult = data._stateEstimator->getResult();
    Vec3<T> pFoot_des_body = seResult.rBody * (ctrl_data->pFoot_des[leg] - seResult.position)  
                - data._quadruped->getHipLocation(leg);
    Vec3<T> vFoot_des_body = seResult.rBody * (ctrl_data->vFoot_des[leg] - seResult.vWorld);
    Vec3<T> qDes;
    // convert
    computeLegAngle(*data._quadruped, pFoot_des_body, &qDes);    
    BCout->qDes = qDes;
    BCout->qdDes = data._legController->datas[leg].J.inverse() * vFoot_des_body;
}


template class FootTask<float>;
template class FootTask<double>;
